/**
 * @file power.cpp
 * @brief Power management class implementation
 * This class is responsible for managing the power of the device.
 * It uses the IP5306 power management IC to manage the power.
 */
#include "power.h"

static const char *TAG_POWER = "Power";
// ADC
static int adc_raw;
static int voltage;
// static variables
SemaphoreHandle_t Power::__i2c_mutex;
bool Power::__do_calibration1_chan0;
charge_state_t Power::charge_state;
adc_cali_handle_t Power::__adc1_cali_chan0_handle;
adc_oneshot_unit_handle_t Power::__adc_handle;

bool Power::__adcCalibrationInit(adc_unit_t unit, adc_channel_t channel, adc_atten_t atten, adc_cali_handle_t *out_handle)
{
    adc_cali_handle_t handle = NULL;
    esp_err_t ret = ESP_FAIL;
    bool calibrated = false;

    if (!calibrated)
    {
        ESP_LOGI(TAG_POWER, "calibration scheme version is %s", "Curve Fitting");
        adc_cali_curve_fitting_config_t cali_config = {
            .unit_id = unit,
            .chan = channel,
            .atten = atten,
            .bitwidth = ADC_BITWIDTH_DEFAULT,
        };
        ret = adc_cali_create_scheme_curve_fitting(&cali_config, &handle);
        if (ret == ESP_OK)
        {
            calibrated = true;
        }
    }
    *out_handle = handle;
    if (ret == ESP_OK)
    {
        ESP_LOGI(TAG_POWER, "Calibration Success");
    }
    else if (ret == ESP_ERR_NOT_SUPPORTED || !calibrated)
    {
        ESP_LOGW(TAG_POWER, "eFuse not burnt, skip software calibration");
    }
    else
    {
        ESP_LOGE(TAG_POWER, "Invalid arg or no memory");
    }

    return calibrated;
}

esp_err_t Power::__readRegister(uint8_t address, uint8_t reg, uint8_t *data)
{
    // sem
    if (xSemaphoreTake(__i2c_mutex, portMAX_DELAY) != pdTRUE)
    {
        ESP_LOGE(TAG_POWER, "Could not take i2c mutex");
        return ESP_FAIL;
    }
    i2c_master_write_read_device(I2C_MASTER_NUM, address, &reg, 1, data, 1, I2C_MASTER_TIMEOUT_MS / portTICK_PERIOD_MS);
    ESP_LOGD(TAG_POWER, "Read from register 0x%02x: 0x%02x", reg, *data);
    xSemaphoreGive(__i2c_mutex);
    return ESP_OK;
}

esp_err_t Power::__writeRegister(uint8_t address, uint8_t reg, uint8_t data)
{
    // sem
    if (xSemaphoreTake(__i2c_mutex, portMAX_DELAY) != pdTRUE)
    {
        ESP_LOGE(TAG_POWER, "Could not take i2c mutex");
        return ESP_FAIL;
    }
    uint8_t write_buf[2] = {reg, data};
    ESP_LOGD(TAG_POWER, "Write to register 0x%02x: 0x%02x", reg, data);
    i2c_master_write_to_device(I2C_MASTER_NUM, address, write_buf, sizeof(write_buf), I2C_MASTER_TIMEOUT_MS / portTICK_PERIOD_MS);
    xSemaphoreGive(__i2c_mutex);
    return ESP_OK;
}

esp_err_t Power::__initI2C(void)
{

    i2c_config_t conf = {
        .mode = I2C_MODE_MASTER,
        .sda_io_num = I2C_MASTER_SDA_IO,
        .scl_io_num = I2C_MASTER_SCL_IO,
        .sda_pullup_en = GPIO_PULLUP_DISABLE,
        .scl_pullup_en = GPIO_PULLUP_DISABLE,
    };
    conf.master.clk_speed = I2C_MASTER_FREQ_HZ;

    i2c_param_config(I2C_MASTER_NUM, &conf);

    return i2c_driver_install(I2C_MASTER_NUM, conf.mode, I2C_MASTER_RX_BUF_DISABLE, I2C_MASTER_TX_BUF_DISABLE, 0);
}

esp_err_t Power::__initIP5306(void)
{
    uint8_t data;
    __readRegister(IP5306_ADDR, IP5306_REG_SYS_CTL0, &data);
    // Enable boost, disable light load switch off
    __writeRegister(IP5306_ADDR, IP5306_REG_SYS_CTL0, (data & 0x50) | BOOST_ENABLE_BIT | BOOST_OUT_BIT);
    __readRegister(IP5306_ADDR, IP5306_REG_SYS_CTL1, &data);
    // Set charge current to 1000mA and charge voltage to 4.2V
    __writeRegister(IP5306_ADDR, IP5306_REG_SYS_CTL1, (data & 0x1D));
    __readRegister(IP5306_ADDR, IP5306_REG_Charger_CTL2, &data);
    data = (data & 0xf0) | (BAT_4_2V) | (ADD_VOLTAGE_28MV);
    __writeRegister(IP5306_ADDR, IP5306_REG_Charger_CTL2, data);
    __readRegister(IP5306_ADDR, IP5306_REG_CHG_DIG_CTL0, &data);
    __writeRegister(IP5306_ADDR, IP5306_REG_CHG_DIG_CTL0, (data & 0xe0) | CURRENT_800MA | CURRENT_200MA);
    // Force Pulse to enable boost
    // IP5306
    gpio_config_t io_conf;
    io_conf.intr_type = GPIO_INTR_DISABLE;
    io_conf.mode = GPIO_MODE_INPUT;
    io_conf.pin_bit_mask = 1ULL << INT_5306;
    io_conf.pull_down_en = GPIO_PULLDOWN_DISABLE;
    io_conf.pull_up_en = GPIO_PULLUP_DISABLE;
    gpio_config(&io_conf);

    gpio_set_level(POWER_KEY, 1);
    gpio_set_level(POWER_KEY, 0);
    // wait for 50ms
    vTaskDelay(50);
    gpio_set_level(POWER_KEY, 1);
    vTaskDelay(500);
    // check if IP5306 is connected
    if (getBoostState() == 0)
    {
        ESP_LOGE(TAG_POWER, "IP5306 not initialized");
        return ESP_FAIL;
    }
    ESP_LOGI(TAG_POWER, "IP5306 initialized successfully");
    return ESP_OK;
}

void Power::__daemonChargeStateCheck(void *pvParameters)
{
    charge_state_t state;
    uint8_t battery_level = 0;
    while (1)
    {
        state = getChargeState();
        battery_level = getBatteryLevel();
        if (state != ERROR)
            charge_state = state;
        else
            ESP_LOGE(TAG_POWER, "Error reading charge state");

        ESP_ERROR_CHECK(adc_oneshot_read(__adc_handle, ADC_CHAN, &adc_raw));
        if (__do_calibration1_chan0)
        {
            ESP_ERROR_CHECK(adc_cali_raw_to_voltage(__adc1_cali_chan0_handle, adc_raw, &voltage));
            ESP_LOGD(TAG_POWER, "BatteryVoltage = %d mv", voltage * 2);
        }

        ESP_LOGD(TAG_POWER, "BatteryLevel = %d", battery_level);
        ESP_LOGD(TAG_POWER, "PowerState = %d", getBoostState());
        ESP_LOGD(TAG_POWER, "Charging = %d", charge_state);
        vTaskDelay(1000 / portTICK_PERIOD_MS);
    }
    vTaskDelete(NULL);
}

uint8_t Power::getBatteryLevel()
{
    uint8_t data;
    __readRegister(IP5306_ADDR, IP5306_REG_READ3, &data);
    if ((data & 0x0F) == 0x02)
        return 0;
    switch (data & 0xF0)
    {
    case 0x00:
        return 100;
    case 0x80:
        return 75;
    case 0xC0:
        return 50;
    case 0xE0:
        return 25;
    case 0xF0:
        return 0;
    default:
        return 0;
    }
}

charge_state_t Power::getChargeState()
{
    static uint8_t data[2];
    esp_err_t err;
    err = __readRegister(IP5306_ADDR, IP5306_REG_READ0, data);
    if (err != ESP_OK)
        return ERROR;
    err = __readRegister(IP5306_ADDR, IP5306_REG_READ1, data + 1);
    if (err != ESP_OK)
        return ERROR;
    if (data[0] & 0x08)
        if (data[1] & 0x08)
            return FULL;
        else
            return CHARGING;
    else
        return NOT_CHARGING;
}

bool Power::getBoostState()
{
    // Read IP5306_REG_SYS_CTL0, get BIT 5
    esp_err_t err;
    uint8_t data;
    err = __readRegister(IP5306_ADDR, IP5306_REG_SYS_CTL0, &data);
    if (err != ESP_OK)
        return false;
    return data & BOOST_ENABLE_BIT;
}

float Power::getWirelessChargeCurrent()
{
    float current;
    uint8_t rx_buf[2];

    __readRegister(WirelessCharge_Device_Addr, IOUT_FILT, &rx_buf[0]);     // IOUT_FILT [1:0]
    __readRegister(WirelessCharge_Device_Addr, IOUT_FILT + 1, &rx_buf[1]); // IOUT_FILT [9:2]

    current = ((((uint16_t)rx_buf[1] << 2) | (rx_buf[0] & 0x03)) & 0x1ff) * 1.953f; // Iout=IOUT_FILT[9:0]*1.953mA
    return current;
}

float Power::getWirelessChargeVoltage()
{
    float voltage;
    uint8_t rx_buf[2];

    __readRegister(WirelessCharge_Device_Addr, VRECT_FILT, &rx_buf[0]);     // VRECT_FILT [1:0]
    __readRegister(WirelessCharge_Device_Addr, VRECT_FILT + 1, &rx_buf[1]); // VRECT_FILT [9:2]

    voltage = ((((uint16_t)rx_buf[1] << 2) | (rx_buf[0] & 0x03)) & 0x1ff) * 9.766f; // VBAT=VRECT_FILT[9:0]*9.766mV
    return voltage;
}

bool Power::getWirelessChargeTemperature()
{
    uint8_t data;
    __readRegister(WirelessCharge_Device_Addr, OTP_FLAG, &data);
    if (data & 0x40)
        return true;
    else
        return false;
}

uint16_t Power::getBatteryVoltage()
{
    return voltage * 2;
}

esp_err_t Power::setBoostState(bool on)
{
    ESP_LOGI(TAG_POWER, "Switching power %s", on ? "on" : "off");
    uint8_t data;
    if (on)
    {
        __readRegister(IP5306_ADDR, IP5306_REG_SYS_CTL0, &data);
        __writeRegister(IP5306_ADDR, IP5306_REG_SYS_CTL0, (data & 0xD0) | BOOST_ENABLE_BIT | BOOST_OUT_BIT);
    }
    else
    {
        __readRegister(IP5306_ADDR, IP5306_REG_SYS_CTL0, &data);
        __writeRegister(IP5306_ADDR, IP5306_REG_SYS_CTL0, (data & 0xD0));
    }
    // Delay for the power to stabilize
    vTaskDelay(50 / portTICK_PERIOD_MS);
    if (getBoostState() == on)
        return ESP_OK;
    else
        return ESP_FAIL;
}

Power::Power()
{
    // sem
    __i2c_mutex = xSemaphoreCreateMutex();
    ESP_ERROR_CHECK_WITHOUT_ABORT(__initI2C());
    ESP_ERROR_CHECK_WITHOUT_ABORT(__initIP5306());
    //-------------ADC1 Init---------------//
    adc_oneshot_unit_init_cfg_t adc_init_config = {
        .unit_id = ADC_UNIT_1,
    };
    ESP_ERROR_CHECK(adc_oneshot_new_unit(&adc_init_config, &__adc_handle));

    //-------------ADC1 Config---------------//
    adc_oneshot_chan_cfg_t config = {
        .atten = ADC_ATTEN,
        .bitwidth = ADC_BITWIDTH_DEFAULT,
    };
    ESP_ERROR_CHECK(adc_oneshot_config_channel(__adc_handle, ADC_CHAN, &config));

    //-------------ADC1 Calibration Init---------------//
    __adc1_cali_chan0_handle = NULL;
    __do_calibration1_chan0 = __adcCalibrationInit(ADC_UNIT_1, ADC_CHAN, ADC_ATTEN, &__adc1_cali_chan0_handle);
    // task
    xTaskCreate(__daemonChargeStateCheck, "taskChargeStateCheck", 4096, NULL, 5, NULL);
}